/* -------------------------------------------------------------------------------
----------------------------------------------------------------------------------
VSC
----------------------------------------------------------------------------------
-------------------------------------------------------------------------------- */
#include <math.h>
//#include <stdio.h>
//#define M_PI 3.14159265358979323846

double abs(double a) {
	if (a < 0) { 
		return -1*a; 
	}
	else {
		return a;
	}
}

int MatrixMatrix(double a[][6], double b[], double c[][6]) {
int ii,ll;

        for (ii=0; ii<6; ii++) {
             for (ll=0; ll<6; ll++) {
				c[ii][ll] = a[ii][ll]*b[ll];
			}
        }

return 0;
}


int MatrixVector(double a[][6], double b[], double c[], double k) {
double sum=0;
int ii,ll;

        for (ii=0; ii<6; ii++) {
                sum = 0;
                for (ll=0; ll<6; ll++) {sum += a[ii][ll]*b[ll];}
                c[ii] = k*sum;
        }

return 0;
}

/* ---<>------------------------------------------------------------------
                        Start of Main Program
----------------------------------------------------------------------- */
extern "C" int VSC(double u[], double M[][6], double r[], double e_dot[], double k[], double c, double epsilon, double alpha) {
double term1a[6][6] = {0};
double term1[6] = {0};
double term2[6] = {0};
double term3[6] = {0};

MatrixMatrix(M,k,term1a);
MatrixVector(term1a,r,term1,1);

term2[0] = c*c*r[0]/(c*abs(r[0])+epsilon);
term2[1] = c*c*r[1]/(c*abs(r[1])+epsilon);
term2[2] = c*c*r[2]/(c*abs(r[2])+epsilon);
term2[3] = c*c*r[3]/(c*abs(r[3])+epsilon);
term2[4] = c*c*r[4]/(c*abs(r[4])+epsilon);
term2[5] = c*c*r[5]/(c*abs(r[5])+epsilon);

MatrixVector(M,e_dot,term3,alpha);

u[0] = term1[0]+term2[0]+term3[0];
u[1] = term1[1]+term2[1]+term3[1];
u[2] = term1[2]+term2[2]+term3[2];
u[3] = term1[3]+term2[3]+term3[3];
u[4] = term1[4]+term2[4]+term3[4];
u[5] = term1[5]+term2[5]+term3[5];

return 0;
}

